#ifndef RIGIDCOORDCALIBRATION_H
#define RIGIDCOORDCALIBRATION_H

#include <Eigen/Core>
#include <memath/matrixd.h>



/**
 * @brief 刚体坐标标定算法
 * 五面体跟踪器在机械臂B中的坐标计算
 */
class RigidCoordCalibration
{
public:
    /// 通过B空间4个点在3个位置的坐标 和 A空间的两个转化矩阵, 计算 A和B之间的矩阵
    static void calRigidMatrix_AfrB(Matrix4d &m_A1frA2, Matrix4d &m_A1frA3, std::vector<Eigen::Vector3d> &pts_B1, std::vector<Eigen::Vector3d> &pts_B2, std::vector<Eigen::Vector3d> &pts_B3, Matrix4d &m_AfrB);
    static void calRigidMatrix_AfrB(Matrix4d &m_A1frA2, Matrix4d &m_A1frA3, Matrix4d &m_A1frA4,
                                    std::vector<Eigen::Vector3d> &pts_B1, std::vector<Eigen::Vector3d> &pts_B2,
                                    std::vector<Eigen::Vector3d> &pts_B3, std::vector<Eigen::Vector3d> &pts_B4,
                                    Matrix4d &m_AfrB);
    static void calRigidMatrix_AfrB(std::vector<Matrix4d> &m_AxfrAy,
                         std::vector<std::vector<Eigen::Vector3d>> &pts_By,
                         Matrix4d &m_AfrB);

    /// 计算旋转J5以后, E坐标系变换的矩阵
    static Matrix4d calMatrixRotateJ5(double angleJ5, double l=94);
    /// 计算旋转J4以后, E坐标系变换的矩阵
    static Matrix4d calMatrixRotateJ4(double angleJ4, double l=102.5);
};

#endif // RIGIDCOORDCALIBRATION_H
